
import lejos.nxt.*;
import lejos.robotics.navigation.Pilot;
import lejos.robotics.navigation.TachoPilot;



public class Lijnvolger {
	
	static LightSensor sensor; 
	static Pilot pilot ;
	

	static int laatsteRichting ;
	static int lijnwaarde;
	static int achtergrond;
	static int treshold;
	
	public static void main(String[] args) {
		

		sensor = new LightSensor(SensorPort.S1);
		pilot = new TachoPilot(5.52012f, 11.1354f, Motor.A, Motor.C, true);
		Motor.A.smoothAcceleration(true);
		Motor.C.smoothAcceleration(true);
		laatsteRichting = 1;
		int zwart = 1; 
		
		// Zet licht van de sensor aan, calibreer
		sensor.setFloodlight(true);
		calibrate();
		
		System.out.println("Start");
		Button.ENTER.waitForPressAndRelease();
		
		
		// Beslissing nemen of lijn wit of zwart is
		if(lijnwaarde > achtergrond)
			zwart = -1;
		
	
		pilot.setTurnSpeed(60);
		
		while(true) {
			// Begin te rijden
			pilot.travel(1000000, true);
			
			//Als de waarde van de lichtsensor de treshold overschrijdt, rijdt de robot niet meer op de lijn : stop
			while(pilot.isMoving()) {
				if(zwart*sensor.readValue() > zwart*treshold)
					pilot.stop();
			}
			
			// Zoek de lijn terug
			vindLijn(zwart);
			}
			
		}
	
	public static void calibrate() {
	
		// Stel de hoogste waarde in (wit)
		System.out.println("Meet wit");
		Button.ENTER.waitForPressAndRelease();
		sensor.calibrateHigh();
		
		// Stel de laagste waarde in (zwart)
		System.out.println("Meet zwart");
		Button.ENTER.waitForPressAndRelease();
		sensor.calibrateLow();
		
		// Meet de lichtwaarde op de lijn
		System.out.println("Meet lijn");
		Button.ENTER.waitForPressAndRelease();
		lijnwaarde = sensor.readValue();
		
		// Meet de lichtwaarde van de achtergrond
		System.out.println("Meet achtergrond");
		Button.ENTER.waitForPressAndRelease();
		achtergrond = sensor.readValue();
		
		// Stel de treshold in : midden tussen de lijnwaarde en de waarde van de achtergrond
		treshold = (lijnwaarde + achtergrond) / 2;
	}
	
	public static void vindLijn(int zwart) {
		
		boolean lijnGevonden = false;
		float hoek = 10;
		while(!lijnGevonden) {
			// Draai in de zelfde richting als de laatste keer
			pilot.rotate(laatsteRichting*(2*hoek - 10), true);

			while(pilot.isMoving())
				// Als de lichtwaarde weer onder de treshold komt , is de lijn gevonden, dus return
				if(zwart*sensor.readValue() < zwart*treshold) {
					lijnGevonden = true;
					pilot.stop();
				}
			
			// Als de lijn niet gevonden is, draai in de andere richting
			if((zwart*sensor.readValue() > zwart*treshold) && !lijnGevonden ) {
				laatsteRichting = -laatsteRichting;
				pilot.rotate(laatsteRichting*(2*hoek),true);
				
				// Stop als lijn gevonden is
				while(pilot.isMoving())
					if(zwart*sensor.readValue() < zwart*treshold) {
						lijnGevonden = true;
						pilot.stop();
					}
				
				// Als de lijn niet gevonden is, vergroot hoek en herhaal
				if(zwart*sensor.readValue() > zwart*treshold && !lijnGevonden) {
					hoek = hoek + 10;
					laatsteRichting = -laatsteRichting; }
				
				else
					lijnGevonden = true;
			}
			
			else {
				lijnGevonden = true;
			}
			
		}
		
		
	}
	
}




		
